/*
 * @Author: LVGRAPE
 * @Date: 2023-09-06 15:17:21
 * @LastEditTime: 2023-10-12 10:30:48
 * @LastEditors: LVGRAPE
 * @Description: 电机驱动，只有正转。可配置频率、占比范围、电机开关。
 * 电机编号与位置：
 * M1(CH4,ADC11) -------- M4(CH3,ADC10)
 * M2(CH1,ADC15) -------- M3(CH2,ADC14)
 * @FilePath: \ZINO_FC_V4\ZINO\hardware\motor\motor.c
 * 要啥没啥，爱咋咋的
 */
#include <rtthread.h>
#include <rtdevice.h>
#include "zino.h"

#define DBG_TAG "motor"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define pwm_dev_name "pwm8"
#define MOTOR_DEV_NAME "motor"
#define MOTOR_DEFAULT_FREQ 50000
#define MOTOR_DEFAULT_DUTY_RANGE 1000

static struct rt_device_pwm* pwm_device = RT_NULL;
struct motor {
    uint32_t freq;
    uint32_t duty[4];
    uint32_t state;
    uint32_t peroid;
    uint32_t dutyRange;
}zinoMotor;
/**
 * @brief 调用系统的pwm8来做为电机驱动。电机输出前的前置操作。
 *
 * @return int
 */
int motor_init(void)
{
    pwm_device = (struct rt_device_pwm*)rt_device_find(pwm_dev_name);
    if (pwm_device == RT_NULL)
    {
        LOG_E("can't find pwm device:%s", pwm_dev_name);
        return RT_ERROR;
    }
    if (!zinoMotor.freq) zinoMotor.freq = MOTOR_DEFAULT_FREQ;
    zinoMotor.peroid = 1000000000UL / zinoMotor.freq;
    zinoMotor.dutyRange = MOTOR_DEFAULT_DUTY_RANGE;
    rt_pwm_set(pwm_device, 1, zinoMotor.peroid, 0);
    rt_pwm_set(pwm_device, 2, zinoMotor.peroid, 0);
    rt_pwm_set(pwm_device, 3, zinoMotor.peroid, 0);
    rt_pwm_set(pwm_device, 4, zinoMotor.peroid, 0);

    LOG_I("motor init success!");
    LOG_I("Freq:%dHz Period:%dns Dutyrang:[0-%d]", zinoMotor.freq, zinoMotor.peroid, zinoMotor.dutyRange);
    return RT_EOK;
}
void motor_set_freq(uint32_t freq)
{
    if (!freq) freq = MOTOR_DEFAULT_FREQ;
    zinoMotor.freq = freq;
    zinoMotor.peroid = 1000000000UL / zinoMotor.freq;
    if (!pwm_device)
    {
        LOG_E("Init motor first!\n");
        return;
    }
    rt_pwm_set_period(pwm_device, 1, zinoMotor.peroid);
}
/**
 * @brief
 *
 * @param mx 1,2,3,4
 * @param duty [0(%0),dutyRange(%100)]
 */
void motor_set_duty(uint8_t mx, uint32_t duty)
{
    if (!pwm_device)
    {
        LOG_E("Init motor first!\n");
        return;
    }
    if (mx < 5 && mx > 0)
    {
        zinoMotor.duty[mx - 1] = duty < zinoMotor.dutyRange ? duty : zinoMotor.dutyRange;
        uint32_t pluse = zinoMotor.peroid * zinoMotor.duty[mx - 1] / zinoMotor.dutyRange;
        LOG_D("M%d pluse:%d p:%d r:%d\n", mx, pluse, zinoMotor.peroid, zinoMotor.dutyRange);
        rt_pwm_set_pulse(pwm_device, mx, pluse);
    }
    else
    {
        LOG_E("Invalid motor index:%d (expected <4)\n", mx);
    }
}
void motor_on_all()
{
    if (!pwm_device)
    {
        motor_init();
    }
    if (pwm_device)
    {
        rt_pwm_enable(pwm_device, 1);
        rt_pwm_enable(pwm_device, 2);
        rt_pwm_enable(pwm_device, 3);
        rt_pwm_enable(pwm_device, 4);
    }
}
void motor_off_all()
{
    if (!pwm_device)  return;
    rt_pwm_disable(pwm_device, 1);
    rt_pwm_disable(pwm_device, 2);
    rt_pwm_disable(pwm_device, 3);
    rt_pwm_disable(pwm_device, 4);
}
void motor_on(uint8_t mx)
{
    if (mx < 5 && mx > 0 && pwm_device)
    {
        rt_pwm_enable(pwm_device, mx);
    }
}
void motor_off(uint8_t mx)
{
    if (mx < 5 && mx > 0 && pwm_device)
    {
        rt_pwm_disable(pwm_device, mx);
    }
}
void motor_set_duty_range(uint32_t range)
{
    zinoMotor.dutyRange = range ? range : 1000;
}
long motor(int argc, char** argv)
{
    if (argc == 4)
    {
        if (rt_strcmp(argv[1], "set") == 0)
        {
            if (rt_strcmp(argv[2], "freq") == 0)
            {
                motor_set_freq(atoi(argv[3]));
                rt_kprintf("Set motor freq:%d\n", atoi(argv[3]));
            }
            else if (rt_strcmp(argv[2], "rang") == 0)
            {
                motor_set_duty_range(atoi(argv[3]));
                rt_kprintf("Set motor duty range:%d\n", atoi(argv[3]));
            }
            else
            {
                motor_set_duty(atoi(argv[2]), atoi(argv[3]));
                rt_kprintf("Set motor %d duty:%d\n", atoi(argv[2]), atoi(argv[3]));
            }
        }

    }
    else if (argc == 2)
    {
        if (rt_strcmp(argv[1], "on") == 0)
        {
            motor_on_all();
            rt_kprintf("Motor on\n");
        }
        else if (rt_strcmp(argv[1], "off") == 0)
        {
            motor_off_all();
            rt_kprintf("Motor off\n");
        }
    }
    else
    {
        rt_kprintf("Usage: \n");
        rt_kprintf("\tmotor on\n");
        rt_kprintf("\tmotor off\n");
        rt_kprintf("\tmotor set freq [freq]      --- for frequency\n");
        rt_kprintf("\tmotor set rang [rang]      --- for duty rang\n");
        rt_kprintf("\tmotor set [channel] [duty] --- for duty\n");

    }
    return RT_EOK;
}
ZINO_APP_EXPORT(motor_init);
MSH_CMD_EXPORT(motor, motor control);